package crtl;

import java.util.HashMap;


/**
 *
 * @author marcel meraner
 */
public abstract class BangBangCtrl extends EPuckCtrl {

	public BangBangCtrl() {
		super();
	}
	
	@Override
	protected abstract boolean finished();

	@Override
	protected abstract Speed navigate();
	
	/**
	 * Calculate the speed for the given direction
	 * 
	 * @param d direction
	 * @param intensity
	 * @return 
	 */
	protected Speed calcSpeed(Direction d, double intensity) {
		double left;
		double right;
		
		double angle = d.getAngle();
		double subAngle = angle % 90;
		
		if (angle < 90) {
			left = 1;
			right = 1 - (subAngle / 90);
			
		} else if (angle < 180) {
			left = 1;
			right = 0 - (subAngle / 90);
			
		} else if (angle < 270) {
			left = 0 - (subAngle / 90);
			right = 1;
			
		} else {
			left = 1 - (subAngle / 90);
			right = 1;
		}
		
		left *= intensity;
		right *= intensity;
		
		try {
			Speed speed = new Speed(left, right);
			return speed;

		} catch (SetSpeedException ex) {
			return null;
		}
	}
	
	/**
	 * Convert a given vector with sensor values into a "user friendly" hashmap
	 * @param s
	 * @return 
	 */
	protected HashMap<Direction, Double> convertSensorValues(double[] s) {
		HashMap<Direction, Double> rv = new HashMap<>();
		for (int index = 0; index < s.length; index++) {
			rv.put(Direction.getByIndex(index), s[index]);
		}
		return rv;
	}
}
